﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace AStar
{
    /// <summary>
    /// 节点信息
    /// </summary>
    public class Node
    {
       public int x;
       public int y;
       public double H;
       public double G;
       public double F;
       public Node Paraent;
    }

    /// <summary>
    /// 地图信息 0表示可以通过 1表示障碍
    /// </summary>
    public class Map
    {
        public int Width;
        public int Height;
        public int[,] MapData;
    }

    public class AStar
    {
        private Map map;

        private Node startNode;
        private Node endNode;
        private Node lastNode;

        private List<Node> openList;
        private List<Node> closeList;
        private List<Node> moveList;
		private List<Node> moveList8;

        public List<Node> MoveList
        {
            get { return moveList; }
            set { moveList = value; }
        }

		public List<Node> MoveList8
		{
			get 
			{ 
				if(moveList8 == null || moveList8.Count <= 0)
					moveList8 = AStarTool.Path4To8(MoveList);
				return moveList8;
			}
		}


        public List<Node> CloseList
        {
            get { return closeList; }
            set { closeList = value; }
        }

        public AStar(Map map)
        {
          
            this.map = map;
        }

		public List<Node> Find4(Node start, Node end)
		{
			Find(start, end);
			return MoveList;
		}

		public List<Node> Find8(Node start, Node end)
		{
			Find(start, end);
			return MoveList8;
		}

        /// <summary>
        /// 查找路线
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        public void Find(Node start, Node end)
        {
            this.startNode = start;
            this.endNode = end;
            openList = new List<Node>();
            closeList = new List<Node>();
            moveList = new List<Node>();
			moveList8 = null;

            SetFG(ref startNode);
            openList.Add(startNode);

            startNode.Paraent = null;

            StartFind();

            GetMoveList();
        }


        private void GetMoveList()
        {
            while (lastNode.Paraent != null)
            {
                moveList.Insert(0, lastNode);
                lastNode = lastNode.Paraent;
            }
        
        }
        private void SetFG(ref Node node)
        {
            node.G = Math.Sqrt( Math.Pow(this.endNode.x - node.x, 2) + Math.Pow(this.endNode.y - node.y, 2) ) ;
            node.F = node.G + node.H;
        }

        /// <summary>
        /// 判断该节点是否为障碍
        /// </summary>
        /// <param name="node"></param>
        /// <returns></returns>
        private bool IsCanGoto(Node node)
        {
            if (node.x>=0&&node.x<map.Width &&node.y>=0&&node.y<map.Height&&map.MapData[node.x, node.y] == 0) return true;
            else return false;
        }

        private void StartFind()
        {
           while(openList.Count > 0)
            {
                Node MinNode=openList[0];
                foreach (Node node in openList)
                {
                    if (MinNode.F > node.F) MinNode = node;
                }
                if (MinNode.x == endNode.x && MinNode.y == endNode.y)
                {
                    lastNode = MinNode;
                    break;
                }

                #region 当前节点的子节点
                Node[] nextNode = new Node[4];
                nextNode[0] = new Node();
                nextNode[0].x = MinNode.x - 0;
                nextNode[0].y = MinNode.y - 1;
                nextNode[0].H = MinNode.H + 1;
                SetFG(ref nextNode[0]);

                nextNode[1] = new Node();
                nextNode[1].x = MinNode.x - 1;
                nextNode[1].y = MinNode.y - 0;
                nextNode[1].H = MinNode.H + 1;
                SetFG(ref nextNode[1]);

                nextNode[2] = new Node();
                nextNode[2].x = MinNode.x - 0;
                nextNode[2].y = MinNode.y + 1;
                nextNode[2].H = MinNode.H + 1;
                SetFG(ref nextNode[2]);

                nextNode[3] = new Node();
                nextNode[3].x = MinNode.x + 1;
                nextNode[3].y = MinNode.y - 0;
                nextNode[3].H = MinNode.H + 1;
                SetFG(ref nextNode[3]);
                #endregion


                #region 判断子节点的信息，将其加入到Open or Close 表中
                bool[] IsInOC = { false, false, false, false };
                for (int i = 0; i < 4; i++)
                {
                    if (IsCanGoto(nextNode[i]))
                    {
                        for (int j = 0; j < openList.Count; j++)
                        {
                            if (nextNode[i].x == openList[j].x && nextNode[i].y == openList[j].y)
                            {
                                if (nextNode[i].F < openList[j].F)
                                {
                                    openList[j] = nextNode[i];
                                }
                                IsInOC[i] = true;
                            }
                        }

                        for (int j = 0; j < closeList.Count; j++)
                        {
                            if (nextNode[i].x == closeList[j].x && nextNode[i].y == closeList[j].y)
                            {
                                if (nextNode[i].F < closeList[j].F)
                                {
                                    closeList.Remove(closeList[j]);
                                    openList.Add(nextNode[i]);
                                }
                                IsInOC[i] = true;
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++)
                {
                    
                    if (IsCanGoto(nextNode[i]))
                    {
                        nextNode[i].Paraent = MinNode;
                        if (IsInOC[i] == false)
                        {
                            openList.Add(nextNode[i]);
                        }
                    }
                }

                #endregion
                openList.Remove(MinNode);
                closeList.Add(MinNode);
                
            }
        }
    }

	public class AStarTool
	{
		public static bool eq(Node n1, Node n2)
		{
			return n1.x == n2.x && n1.y == n2.y;
		}

		public static bool eq(Node n1, Node n2, int off_x, int off_y)
		{
			return (n1.x == n2.x + off_x) && (n1.y == n2.y + off_y);
		}

		public static bool isNear(Node n1, Node n2)
		{
			// x:-1, y:-1
			if(eq (n1, n2, -1, -1))
				return true;
			// x:-1, y:0
			else if(eq (n1, n2, -1, 0))
				return true;
			// x:-1, y:+1
			else if(eq (n1, n2, -1, +1))
				return true;
			// x:0, y:-1
			else if(eq (n1, n2, 0, -1))
				return true;
			// x:0, y:0
			else if(eq (n1, n2, 0, 0))
				return true;
			// x:0, y:+1
			else if(eq (n1, n2, 0, +1))
				return true;
			// x:+1, y:-1
			else if(eq (n1, n2, +1, 0))
				return true;
			// x:+1, y:0
			else if(eq (n1, n2, +1, 0))
				return true;

			return false;
			// x:+1, y:+1
		}

		public static string ToString(Node n)
		{
			return "("+n.x+":"+n.y+")";
		}

		public static string ToString(List<Node> list)
		{
			StringBuilder sb = new StringBuilder();
			foreach(Node node in list)
			{
				sb.Append(AStarTool.ToString(node)).Append(",");
			}
			if(sb.Length > 1)
				sb.Remove(sb.Length - 1, 1);
			return sb.ToString();
		}

		public static List<Node> Path4To8(List<Node> list)
		{
			if(list == null || list.Count < 3)
				return list;

			List<Node> r2 = new List<Node>();

			int Count = list.Count;
			for(int i = 0; i < Count; i ++)
			{
				Node n1 = list[i];
				if(i >= Count - 1) {
					r2.Add(n1);
					return r2;
				}

				Node n2 = list[i + 1];
				if(i >= Count - 2) {
					r2.Add(n1);
					r2.Add(n2);
					return r2;
				}

				Node n3 = list[i + 2];

				r2.Add(n1);
				i = i + 2; // jump 2
				if(!isNear(n1, n3)) {
					r2.Add(n2);
				}
				r2.Add(n3);
			}

			return r2;
		}
	}

	public class AStarTest
	{
		
		public List< Node> Test()
		{
			// init
			Node start = new Node();
			Node end = new Node();
			Map map = new Map();
			map.Width = 50;
			map.Height = 50;
			map.MapData = new int[50, 50];
			for (int i = 0; i < 50; i++)
			{
				for (int j = 0; j < 50; j++)
				{
					map.MapData[i, j] = 0;
				}
			}

			start.x = 1;
			start.y = 1;

			end.x = 30;
			end.y = 20;

			AStar astar = new AStar(map);
			astar.Find(start, end);
			
			List< Node> list = astar.MoveList;
			return list;
		}
	}
}



/*A Star算法是一智能找最短路径算法(下面简称A算法), 与 Dijkstra算法相比，A算法访问的节点比较少，因此可以缩短搜索时间。他的算法思想是：

这里有公式f

         最终路径长度f = 起点到该点的已知长度h + 该点到终点的估计长度g。

         O表(open):

                待处理的节点表。

         C表(close):

                已处理过的节点表。

算法流程:

1. 从起点开始，起点的f = 1 + g,  1表示此节点已走过的路径是1，g是此节点到终点的估计距离， 放入链表O中。

可以假设g值的计算使用勾股定理公式来计算此点到终点的直线距离。

2. 当O不为空时，从中取出一个最小f值的节点x。

3.如果x等于终点，找到路径，算法结束。否则走第4步.

4. 遍历x的所有相邻点，对所有相邻点使用公式f,计算出f值后，

先检查每个相邻节点y是否在链表O和C中，

如果在O中的话的话，更新y节点的f值，保留最小的f值，

如果在C中的话，并且此时f值比C中的f值小，则更新f值，将y节点从C中移到O中。否则不做操作。

如果不在以上两表中，按最小顺序排序将y插入链表O。最后将x插入C表中
*/